#include "postprocess/kalman_filter/base_kalmanfilter.h"

#include <algorithm>

BaseKalmanFilter::BaseKalmanFilter() {}

BaseKalmanFilter::~BaseKalmanFilter() {}

void BaseKalmanFilter::Init(int dynam_param_num, int measure_param_num,
                            int control_param_num) {
  A_ = Eigen::MatrixXf::Identity(dynam_param_num, dynam_param_num);

  Q_ = Eigen::MatrixXf::Identity(dynam_param_num, dynam_param_num);

  H_ = Eigen::MatrixXf::Zero(measure_param_num, dynam_param_num);

  R_ = Eigen::MatrixXf::Identity(measure_param_num, measure_param_num);

  P_ = Eigen::MatrixXf::Identity(dynam_param_num, dynam_param_num);

  State_ = Eigen::VectorXf::Zero(dynam_param_num);

  dynam_param_num_ = dynam_param_num;
  measure_param_num_ = measure_param_num;
  control_param_num_ = control_param_num;
}
